﻿/**
 * @file scb_can.c
 * @author your name (you@domain.com)
 * @brief 
 * @version 0.1
 * @date 2020-09-08
 * 
 * @copyright Copyright (c) 2020
 * 
 */
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <pthread.h>
#include "motion.h"
#include <math.h>
#include "rtapi/rtapi.h"
#include "hal/hal.h"
#include "errcode.h"
#include "controlcan/controlcan.h"
#include "config.h"
#include <time.h>
#include <stdlib.h>
#include "unistd.h"
#include <string.h>
#include "scb_can.h"
#include "mot_priv.h"
#include "errcode.h"
#include "controlDefine.h"

#define SCB_PRINT(FMT, ...) rtapi_print(FMT, ##__VA_ARGS__)
#define SCB_DEBUG(FMT, ...) rtapi_print(FMT, ##__VA_ARGS__)

extern zucmot_config_t* zucmotConfig;

// #define DEBUG_OUTPUT
#ifdef DEBUG_OUTPUT
#define ZUC_INTER_CAN_PRINT(fmt, args...) rtapi_print(fmt, ##args)
#else
#define ZUC_INTER_CAN_PRINT(fmt, args...)
#endif

//static int comm_stop_flag = 0;
static int cab_type = 2;

static void zuc_inter_can_rx_0x193(void);
static void zuc_inter_can_rx_0x292(void);
static void zuc_inter_can_rx_0x298(void);
static void zuc_inter_can_rx_0x592(void);
/*
 * message statue management
 * rx message online status
 * tx message enable status
*/
static zuc_inter_can_msg_sts_t zuc_inter_can_msg_sts[ZUC_INTER_CAN_MSG_ALL];

// static zuc_scb_do_ao_feedback_t zuc_scb_do_ao_feedback;
// static zuc_scb_ai_t zuc_scb_ai;
// static zuc_scb_di_t zuc_scb_di;
static zuc_scb_req_resp_t zuc_scb_req;
static zuc_scb_req_resp_t zuc_scb_resp;
static zuc_scb_ao_t zuc_scb_ao;
static zuc_scb_do_t zuc_scb_do;
static zuc_scb_safety_do_ctl_t scb_safety_do_ctl;
/**/
// static unsigned char zuc_scb_type_init = 0;
/*'P':0x50:PSCB
	:0:SCB
*/
// static unsigned char zuc_scb_type = 0;

/*
 * command message send to bp
*/
// static zuc_bp_can_cmd_t zuc_bp_can_cmd;

// static zuc_bp_can_status_t zuc_bp_can_status;
/**
 * @brief 
 * 
 */
static VCI_CAN_OBJ zuc_internal_comm_recv_msg[SCB_CAN_RX_MAX_CNT];
static VCI_CAN_OBJ zuc_internal_comm_send_msg[SCB_CAN_TX_MAX_CNT];

zucab_internal_info_t cab_internal_info = {0};

int32_t index_2122[CABNUM] = {0x2122, 0x200};
int32_t index_2125[CABNUM] = {0x2125, 0x202};
int32_t index_2126[CABNUM] = {0x2126, 0x203};
int32_t index_2127[CABNUM] = {0x2127, 0x204};
int32_t index_2128[CABNUM] = {0x2128, 0x205};
int32_t index_2129[CABNUM] = {0x2129, 0x206};
int32_t index_2148[CABNUM] = {0x2148, 0x100};
int32_t index_2149[CABNUM] = {0x2149, 0x101};
int32_t index_214A[CABNUM] = {0x214A, 0x102};
int32_t index_214B[CABNUM] = {0x214B, 0x000};
int32_t index_214D[CABNUM] = {0x214D, 0x20B};
int32_t index_215B[CABNUM] = {0x215B, 0x308};
int32_t index_215C[CABNUM] = {0x215C, 0x309};
int32_t index_215D[CABNUM] = {0x215D, 0x109};
int32_t index_215E[CABNUM] = {0x215E, 0x10A};
int32_t index_215F[CABNUM] = {0x215F, 0x30A};
int32_t index_3149[CABNUM] = {0x3149, 0x309};
int32_t index_314C[CABNUM] = {0x314C, 0x402};
int32_t index_314D[CABNUM] = {0x314D, 0x403};
int32_t index_314E[CABNUM] = {0x314E, 0x30A};
int32_t index_3148[CABNUM] = {0x3148, 0x308};
int32_t index_314A[CABNUM] = {0x314A, 0x400};
int32_t index_314B[CABNUM] = {0x314B, 0x401};
int32_t index_3152[CABNUM] = {0x3152, 0x000};
int32_t index_2160[CABNUM] = {0x2160, 0x000};  //get cabVoltage
int32_t index_314F[CABNUM] = {0x314F, 0x000};  //set cabVoltage
int32_t index_316B[CABNUM] = {0x316B, 0x000};  // set RC version 控制器版本号
/**
 * @brief 
 * 
 */
static void zuc_inter_comm_debug_show_infor(void)
{
#if 0

    ZUC_INTER_CAN_PRINT("scb_di: user CI->0x%x-DI->0x%x-LOCAL->0x%x--LOCAL->0x%x-UHI-pules-H->0x%x-UHI-pules-L->0x%x-UHI-speed-H->0x%x-UHI-speed-L->0x%x\n",
                        cab_internal_info.scb_di_uhi_info.msg_data[0],
                        cab_internal_info.scb_di_uhi_info.msg_data[1],
                        cab_internal_info.scb_di_uhi_info.msg_data[2],
                        cab_internal_info.scb_di_uhi_info.msg_data[3],
                        cab_internal_info.scb_di_uhi_info.msg_data[4],
                        cab_internal_info.scb_di_uhi_info.msg_data[5],
                        cab_internal_info.scb_di_uhi_info.msg_data[6],
                        cab_internal_info.scb_di_uhi_info.msg_data[7]);
    ZUC_INTER_CAN_PRINT("Estop:BP_1 = %d, BP_2 = %d\n",
                        cab_internal_info.scb_di_uhi_info.di_signal.local_bp_estop_1,
                        cab_internal_info.scb_di_uhi_info.di_signal.local_bp_estop_2);
    ZUC_INTER_CAN_PRINT("Estop:USR_1 = %d, USR_2 = %d\n",
                        cab_internal_info.scb_di_uhi_info.di_signal.local_usr_estop_1,
                        cab_internal_info.scb_di_uhi_info.di_signal.local_usr_estop_2);
#endif
}
/**
 * @brief 
 * 
 */
zuc_inter_can_msg_t zuc_inter_can_msg_table[ZUC_INTER_CAN_MSG_ALL] = {
    // {0x191, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_RX, 8, 1000, zuc_bp_can_status.data, zuc_inter_can_rx_0x191},
    // {0x192, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_RX, 8, 1000, cab_internal_info.scb_di_uhi_info.msg_data, zuc_inter_can_rx_0x192},
    {0x193, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_RX, 8, 1000, cab_internal_info.scb_safety_io_info.msg_data, zuc_inter_can_rx_0x193},
    {0x292, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_RX, 8, 1000, cab_internal_info.scb_ai_uhi_info.msg_data, zuc_inter_can_rx_0x292},
    {0x298, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_RX, 8, 1000, cab_internal_info.scb_do_ao_feedback_info.msg_data, zuc_inter_can_rx_0x298},
    {0x592, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_RX, 0, 1000, zuc_scb_resp.msg_data, zuc_inter_can_rx_0x592},

    // {0x211, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_TX, 8, 125, zuc_bp_can_cmd.data, NULL},
    {0x212, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_TX, 8, 125, zuc_scb_do.msg_data, NULL},
    {0x293, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_TX, 8, 125, scb_safety_do_ctl.msg_data, NULL},
    {0x312, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_TX, 8, 125, zuc_scb_ao.msg_data, NULL},
    {0x612, ZUC_INTER_CAN_MSG_TYPE_STD, ZUC_INTER_CAN_MSG_DIR_TX, 0, 125, zuc_scb_req.msg_data, NULL},

};
/**
 * @brief 
 * 	find the rx message position in zuc_inter_can_msg_table[]
 */
static int zuc_check_rx_mesg_position(int id)
{
    int i;
    for (i = 0; i < ZUC_INTER_CAN_MSG_ALL; i++)
    {
        if (zuc_inter_can_msg_table[i].can_msg_id == id)
        {
            return i;
        }
    }
    return ZUC_INTER_CAN_MSG_ALL;
}

/*
 * bp message management
 * bp IO status
*/
#define ZUC_BP_SHORT_PRESS_TIME (15)
/**
 * @brief 
 * 	button panel message handle
 *  this will analysis the signal on button panel
 * 	like:
 * 		shutdown
 * 		lock/unlock
 * 		pause/resume
 * 		terminate/run
 * 		power on/off robot
 * 		enable/disbale robot
 * 	...
 */

/**
 * @brief 
 *	cyclic message from SCB
 *	DI & UHI signal input for SCB 
 *  DI signal for PSCB  SCB反馈DI状态报文
 *  period:8ms
 */
// static void zuc_inter_can_rx_0x192(void)
// {
//     if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x192].msg_recv_cnt < zuc_inter_can_msg_table[ZUC_INTER_CAN_0x192].period)
//     {
//         zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x192].msg_recv_cnt += zuc_inter_can_msg_table[ZUC_INTER_CAN_0x192].period;
//     }
//     else
//     {
//         zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x192].msg_offline_cnt = 0;
//         /* message connection status: On-line
// 		 *
// 		 */
//         zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x192].msg_offline_sts = 0;
//     }
// }

static void zuc_inter_can_rx_0x193(void)
{
    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x193].msg_recv_cnt < zuc_inter_can_msg_table[ZUC_INTER_CAN_0x193].period)
    {
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x193].msg_recv_cnt += zuc_inter_can_msg_table[ZUC_INTER_CAN_0x193].period;
    }
    else
    {
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x193].msg_offline_cnt = 0;
        /* message connection status: On-line
		 *
		 */
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x193].msg_offline_sts = 0;
    }
}

/**
 * @brief 
 * 	cyclic message from SCB
 *  AIO signal for SCB
 * 	configure  status for PSCB
 *  period:8ms
 */
static void zuc_inter_can_rx_0x292(void)
{
    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x292].msg_recv_cnt < zuc_inter_can_msg_table[ZUC_INTER_CAN_0x292].period)
    {
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x292].msg_recv_cnt += zuc_inter_can_msg_table[ZUC_INTER_CAN_0x292].period;
    }
    else
    {
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x292].msg_offline_cnt = 0;
        /* message connection status: On-line
		 *
		 */
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x292].msg_offline_sts = 0;
    }
}
/**
 * @brief 
 * 	cyclic message from SCB
 * 	DO  feedback for SCB
 * 	None for PSCB
 */
static void zuc_inter_can_rx_0x298(void)
{
    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x298].msg_recv_cnt < zuc_inter_can_msg_table[ZUC_INTER_CAN_0x298].period)
    {
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x298].msg_recv_cnt += zuc_inter_can_msg_table[ZUC_INTER_CAN_0x298].period;
    }
    else
    {
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x298].msg_offline_cnt = 0;
        /* message connection status: On-line
		 *
		 */
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x298].msg_offline_sts = 0;
    }
}

/**
 * @brief 
 * 
 * @param object_index 
 */
// 读上来的值更新到相应的状态里面去
// 592在使用
static void zuc_update_scb_sdos(int object_index)
{
    // 2字节与4字节的处理
    int obj_val = 0;
    if (cab_type == ISCABV3)
    {
        obj_val = (int)((zuc_scb_resp.req_signal.ob_data_1 << 24) + (zuc_scb_resp.req_signal.ob_data_2 << 16) + (zuc_scb_resp.req_signal.ob_data_3 << 8) +
                        zuc_scb_resp.req_signal.ob_data_4);
    }
    else
    {
        obj_val = ((int)(zuc_scb_resp.req_signal.ob_data_1) << 8) + zuc_scb_resp.req_signal.ob_data_2;
    }

    if (object_index == index_2122[cab_type])  //风扇电流
    {
        cab_internal_info.fan_current = obj_val;
    }
    else if (object_index == index_2125[cab_type])  //24V电压采样
    {
        cab_internal_info.pwr_24v_v = obj_val;
    }
    else if (object_index == index_2126[cab_type])  //PDU温度
    {
        cab_internal_info.pdu_temperature = obj_val;
    }
    else if (object_index == index_2127[cab_type])  //机器人电流
    {
        cab_internal_info.robot_current = obj_val;
    }
    else if (object_index == index_2128[cab_type])  //机器人电压
    {
        cab_internal_info.robot_voltage = obj_val;
    }
    else if (object_index == index_2129[cab_type])  //用户IO电流
    {
        cab_internal_info.user_io_current = obj_val;
    }
    else if (object_index == index_2148[cab_type])  //硬件版本号
    {
        cab_internal_info.hw_version = obj_val;
    }
    else if (object_index == index_2149[cab_type])  //软件版本号大版本号
    {
        cab_internal_info.sw_major_version = obj_val;
    }
    else if (object_index == index_214A[cab_type])  //软件版本号小版本号
    {
        cab_internal_info.sw_minor_version = obj_val;
    }
    else if (object_index == index_214D[cab_type])  //机器人功率
    {
        cab_internal_info.robot_power = obj_val;
    }
    else
    {
        // 4 bytes response
        obj_val = (int)((zuc_scb_resp.req_signal.ob_data_1 << 24) + (zuc_scb_resp.req_signal.ob_data_2 << 16) + (zuc_scb_resp.req_signal.ob_data_3 << 8) +
                        zuc_scb_resp.req_signal.ob_data_4);
        if (object_index == index_215B[cab_type])  //输入安全DI配置
        {
            cab_internal_info.sf_input_cfg_fb = obj_val;
        }
        else if (object_index == index_215C[cab_type])  //输出安全DO配置
        {
            cab_internal_info.sf_output_cfg_fb = obj_val;
        }
        else if (object_index == index_215D[cab_type])  //本次工控机供电时间（单位：s）
        {
            cab_internal_info.curr_run_time = obj_val;
        }
        else if (object_index == index_215E[cab_type])  //工控机累计供电时间（单位：s）
        {
            cab_internal_info.accu_run_time = obj_val;
        }
        else if (object_index == index_215F[cab_type])  //控制柜序列号
        {
            cab_internal_info.cabinet_id_fb = obj_val;
        }
        else if (object_index == index_2160[cab_type])  //控制柜供电电压（单位：V）
        {
            cab_internal_info.curCabVoltage = obj_val;
        }
        else
        {
            /*not suppoted**/
        }
    }
}

/**
 * random message from SCB
 * response of the SDO request
 * 
 */
static void zuc_inter_can_rx_0x592(void)
{
    int object_index;
    if (zuc_scb_resp.req_signal.node_index == 0x12)
    {
        object_index = zuc_scb_resp.req_signal.ob_index_h * 256 + zuc_scb_resp.req_signal.ob_index_l;
        /**
		 * @brief Construct a new if object
		 * 	version status:
		 *  distinguish the SCB and PSCB
		 */
        if (object_index == index_214B[cab_type])
        {
            cab_internal_info.sw_release_sts = ((int)(zuc_scb_resp.req_signal.ob_data_1) << 8) + zuc_scb_resp.req_signal.ob_data_2;
            if (zuc_scb_resp.req_signal.ob_data_1 == 'P' || zuc_scb_resp.req_signal.ob_data_1 == 0)
            {
                cab_internal_info.p_scb_type = zuc_scb_resp.req_signal.ob_data_1;
            }
        }
        if (object_index == index_3148[cab_type])
        {
            cab_internal_info.set_safety_config_req.safetyDI_param = 0;
            cab_internal_info.sf_input_cfg_fb = (int)((zuc_scb_resp.req_signal.ob_data_1 << 24) + (zuc_scb_resp.req_signal.ob_data_2 << 16) +
                                                      (zuc_scb_resp.req_signal.ob_data_3 << 8) + zuc_scb_resp.req_signal.ob_data_4);
            // rtapi_print("Set safety DI configuration to SCB successfully!\n");
        }

        if (object_index == index_3149[cab_type])
        {
            cab_internal_info.set_safety_config_req.safetyDO_param = 0;
            cab_internal_info.sf_output_cfg_fb = (int)((zuc_scb_resp.req_signal.ob_data_1 << 24) + (zuc_scb_resp.req_signal.ob_data_2 << 16) +
                                                       (zuc_scb_resp.req_signal.ob_data_3 << 8) + zuc_scb_resp.req_signal.ob_data_4);
            // rtapi_print("Set safety DO configuration to SCB successfully!\n");
        }

        if (object_index == index_314C[cab_type])
        {
            cab_internal_info.set_common_config_req.TCP_offset_param = 0;
            // rtapi_print("Set the TCP offset configuration to SCB successfully!\n");
        }

        if (object_index == index_314D[cab_type])
        {
            cab_internal_info.set_safety_config_req.robot_power_limit_param = 0;
            // rtapi_print("Set the robot power limit configuration to SCB successfully!\n");
        }

        if (object_index == index_3152[cab_type])
        {
            cab_internal_info.set_safety_config_req.drag_tcp_speed_limit_param = 0;
        }

        if (object_index == index_314E[cab_type])
        {
            cab_internal_info.set_common_config_req.cab_id_param = 0;
            // rtapi_print("Set the cab id configuration to SCB successfully!\n");
        }

        if (object_index == index_314F[cab_type])
        {
            cab_internal_info.set_common_config_req.cab_voltage_param = 0;
            // rtapi_print("Set the cab voltage configuration to SCB successfully!\n");
        }

        if (object_index == index_316B[cab_type])
        {
            cab_internal_info.set_common_config_req.RC_version_param = 0;
        }

        if (cab_internal_info.p_scb_type == SCB || cab_internal_info.p_scb_type == PSCB)
        {
            zuc_update_scb_sdos(object_index);
        }
    }
    else
    { /*response is not from SCB*/
    }
}
/**
 * @brief 
 * initialize the message info before sending operations 
 */
void zuc_init_tx_msg_parameters(void)
{
    /*
	 * cyclic send message: 0x211,0x212, 0x312,  
	 * 0x612
	 */
    zuc_internal_comm_send_msg[0].ID = 0x211;
    zuc_internal_comm_send_msg[0].SendType = 0;
    zuc_internal_comm_send_msg[0].RemoteFlag = 0;
    zuc_internal_comm_send_msg[0].ExternFlag = 0;
    zuc_internal_comm_send_msg[0].DataLen = 8;

    zuc_internal_comm_send_msg[1].ID = 0x212;
    zuc_internal_comm_send_msg[1].SendType = 0;
    zuc_internal_comm_send_msg[1].RemoteFlag = 0;
    zuc_internal_comm_send_msg[1].ExternFlag = 0;
    zuc_internal_comm_send_msg[1].DataLen = 8;

    zuc_internal_comm_send_msg[2].ID = 0x312;
    zuc_internal_comm_send_msg[2].SendType = 0;
    zuc_internal_comm_send_msg[2].RemoteFlag = 0;
    zuc_internal_comm_send_msg[2].ExternFlag = 0;
    zuc_internal_comm_send_msg[2].DataLen = 8;

    zuc_internal_comm_send_msg[3].ID = 0x293;
    zuc_internal_comm_send_msg[3].SendType = 0;
    zuc_internal_comm_send_msg[3].RemoteFlag = 0;
    zuc_internal_comm_send_msg[3].ExternFlag = 0;
    zuc_internal_comm_send_msg[3].DataLen = 8;

    zuc_internal_comm_send_msg[4].ID = 0x612;
    zuc_internal_comm_send_msg[4].SendType = 0;
    zuc_internal_comm_send_msg[4].RemoteFlag = 0;
    zuc_internal_comm_send_msg[4].ExternFlag = 0;
    zuc_internal_comm_send_msg[4].DataLen = 8;
}
static int handleScbInitState(int recvLen)
{
    /**
    * 正常情况下，每个周期收到的数据为SCB的AODOAIDI的周期性反馈报文、SCB的随机读写响应报文，以及手柄反馈报文
	* 对于SCB而言，差不多4条；对于PSCB，差不多3条（少了AI反馈）
	* 初始化状态下，CAN缓存可能较多，远超过上述的条数，所以需要清除缓存报文，直到单个周期收到的数据在合理范围内
    */
    // SCB_DEBUG("[SCB_CAN] INIT:recv %d\n", recvLen);
    const int recvLenWhenCommuStable = 10;
    const int recvNormalThresholds = 10;
    static int retryCnt = 0, recvNormalCnt = 0;

    if (recvNormalCnt < recvNormalThresholds)
    {
        if (recvLen > recvLenWhenCommuStable || recvLen <= 0)
        {
            VCI_ClearBuffer(VCI_USBCAN2, 0, 1);
            retryCnt++;
            if (retryCnt < 125)
            {
                return SCB_CAN_STA_INIT;
            }
            else
            {
                recvNormalCnt = retryCnt = 0;
                reportError(CAN_DEVICE_INIT_FAILED, "scb can init failed");
                return SCB_CAN_STA_OP;  //500个周期内都没变正常，则报错。但切换到OP状态按照老时序走。
            }
        }
        else
        {
            recvNormalCnt++;
        }
        return SCB_CAN_STA_INIT;
    }
    recvNormalCnt = retryCnt = 0;
    return SCB_CAN_STA_OP;
}
/**
 * @brief 
 * 		get the recieved message from CAN card
 * 		update the connectio0n status
 * 		sync the data
 */
static int zuc_inter_comm_mesg_rx()
{
    static int scbCanState = SCB_CAN_STA_INIT;

    int reclen = VCI_Receive(VCI_USBCAN2, 0, 1, zuc_internal_comm_recv_msg, SCB_CAN_RX_MAX_CNT, -1);
    if (scbCanState == SCB_CAN_STA_INIT)
    {
        scbCanState = handleScbInitState(reclen);
    }
    else if (scbCanState == SCB_CAN_STA_OP)
    {
        if (reclen > 0)
        {
            for (int i = 0; i < reclen; i++)
            {
                /**
			 * @brief 
			 * check position to determine the data buffer and recv handle function
			 */
                int mesg_position = zuc_check_rx_mesg_position(zuc_internal_comm_recv_msg[i].ID);

                if (mesg_position >= ZUC_INTER_CAN_0x191 && mesg_position <= ZUC_INTER_CAN_0x592)
                {
                    for (int j = 0; j < zuc_internal_comm_recv_msg[i].DataLen; j++)
                    {
                        zuc_inter_can_msg_table[mesg_position].msg_data[j] = zuc_internal_comm_recv_msg[i].Data[j];
                    }
                    zuc_inter_can_msg_table[mesg_position].rx_handler();
                }
                else
                {
                    /* unexpected message
				*  nothing should be done
				*  just ignore the message
				*/
                }
            }
        }
        else
        {
        }
    }
    return scbCanState;
}

/**
 * @brief 
 * 
 * @param ob_index 
 * @param subindex 
 * 
 * @return int 
 * 		0xFFFF0000: busy
 *      0: ready
 */

// ob_index就是索引  send index to scb   612在使用
int zuc_inter_comm_query_data_scb(int ob_index, int subindex)
{
    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag)
    {
        return 0xFFFF0000;
    }
    else
    {
        /*transmition should be performed
		*response should be confirmed
		**/
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag = 1;
        /*node id*/
        zuc_internal_comm_send_msg[4].Data[0] = 0x12;
        /*index*/
        zuc_internal_comm_send_msg[4].Data[1] = (unsigned char)((ob_index & 0xFF00) >> 8);
        zuc_internal_comm_send_msg[4].Data[2] = (unsigned char)(ob_index & 0xFF);
        /*sub index*/
        zuc_internal_comm_send_msg[4].Data[3] = subindex;  //需要改成读状态

        return 0;
    }
}

/**
 * @brief 
 * 
 * @param ob_index 
 * @param subindex 
 * @param data 
 * @return int
 * 		0xFFFF0000: busy
 *      0: ready 
 */
// 没有被调用
int zuc_inter_comm_set_data_scb(int ob_index, int subindex, int data)
{
    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag)
    {
        return 0xFFFF0000;
    }
    else
    {
        /*transmition should be performed
		*response should be confirmed
		**/
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag = 1;

        /*node id*/
        zuc_internal_comm_send_msg[4].Data[0] = 0x12;
        /*index*/
        zuc_internal_comm_send_msg[4].Data[1] = (unsigned char)((ob_index & 0xFF00) >> 8);
        zuc_internal_comm_send_msg[4].Data[2] = (unsigned char)(ob_index & 0xFF);
        /*sub index*/
        zuc_internal_comm_send_msg[4].Data[3] = subindex;

        zuc_internal_comm_send_msg[4].Data[4] = (unsigned char)((data & 0xFF00) >> 8);
        zuc_internal_comm_send_msg[4].Data[5] = (unsigned char)(data & 0xFF);
        return 0;
    }
}
/**
 * @brief 
 * 		TX message management,
 * 		cyclic:
 * 			0x312
 * 			0x212
 * 		random:
 * 			0x612
 * 		
 */
static void zuc_inter_comm_mesg_tx(void)
{
    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag)
    {
        /*0x612 is enable
		 */
        if (VCI_Transmit(VCI_USBCAN2, 0, 1, zuc_internal_comm_send_msg, 5) == 5)
        {
            cab_internal_info.inter_can_bsof_sts = 0;
        }
        else
        {
            cab_internal_info.inter_can_bsof_sts = 1;
            ZUC_INTER_CAN_PRINT("[INTERNAL CAN]:>> internal can bus off on 4 frames \n");
        }
        /*clear tx_en_flag*/
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag = 0;
    }
    else
    {
        if (VCI_Transmit(VCI_USBCAN2, 0, 1, zuc_internal_comm_send_msg, 4) == 4)
        {
            cab_internal_info.inter_can_bsof_sts = 0;
        }
        else
        {
            cab_internal_info.inter_can_bsof_sts = 1;
            ZUC_INTER_CAN_PRINT("[INTERNAL CAN]:>> internal can bus off on 3 frames \n");
        }
    }
}
/**
 * @brief 
 * connection manager 通信检测
 * message communication lost monitor
 */
static void zuc_inter_comm_manage(void)
{
    int i;
    for (i = 0; i < ZUC_INTER_CAN_0x592; i++)
    {
        zuc_inter_can_msg_sts[i].msg_offline_cnt += 8;
        if (zuc_inter_can_msg_sts[i].msg_offline_cnt > zuc_inter_can_msg_table[i].timeout)
        {
            zuc_inter_can_msg_sts[i].msg_offline_sts = 1;
            zuc_inter_can_msg_sts[i].msg_recv_cnt = 0;
        }
    }

    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x192].msg_offline_sts)
    {
        if (cab_internal_info.scb_offline_sts == 0)
            ZUC_INTER_CAN_PRINT("[INTERNAL CAN]:>> SCB off line from IPC\n");
        cab_internal_info.scb_offline_sts = 1;
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag = 0;
    }
    else
    {
        cab_internal_info.scb_offline_sts = 0;
    }

    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x191].msg_offline_sts)
    {
        cab_internal_info.bp_offline_sts = 1;
    }
    else
    {
        cab_internal_info.bp_offline_sts = 0;
    }
}
/**
 * @brief  set 输入安全IO TCP Offset Y...
 * 设置scb参数  需要填写读写状态
 */
static int zuc_set_scb_para(int index, int subindex, unsigned int data)
{
    int ret = 0;
    // fixmecabv3  index range check
    if (zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag)
    {
        ret = 0xFFFF0000;
    }
    else
    {
        /*transmition should be performed
		*response should be confirmed
		**/
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag = 1;
        /*node id*/
        zuc_internal_comm_send_msg[4].Data[0] = 0x12;
        /*index*/
        zuc_internal_comm_send_msg[4].Data[1] = (unsigned char)((index & 0xFF00) >> 8);
        zuc_internal_comm_send_msg[4].Data[2] = (unsigned char)(index & 0xFF);
        /*sub index*/
        zuc_internal_comm_send_msg[4].Data[3] = subindex;  //写状态
        zuc_internal_comm_send_msg[4].Data[4] = (unsigned char)(data >> 24);
        zuc_internal_comm_send_msg[4].Data[5] = (unsigned char)(data >> 16);
        zuc_internal_comm_send_msg[4].Data[6] = (unsigned char)(data >> 8);
        zuc_internal_comm_send_msg[4].Data[7] = (unsigned char)(data >> 0);
    }
    return ret;
}

static int check_scb_version()
{
    //can通信启动正常后2s进行一次check，此后每不在进行check
    static int check_count = 0;
    if (check_count > TIMER_2S)
    {
        return 0;
    }
    else if (check_count < TIMER_2S)
    {
        check_count++;
        return 0;
    }
    check_count++;

    unsigned int hardware_version = ((cab_internal_info.hw_version >> 8) & 0xFF) - '0';
    unsigned int scb_version = (((cab_internal_info.sw_major_version >> 8) & 0xFF) - '0') * 1000;
    scb_version = scb_version + (((cab_internal_info.sw_major_version) & 0xFF) - '0') * 100;
    scb_version = scb_version + (((cab_internal_info.sw_minor_version >> 8) & 0xFF) - '0') * 10;
    scb_version = scb_version + ((cab_internal_info.sw_minor_version & 0xFF) - '0');
    int ret = 0;
    if (cab_internal_info.p_scb_type == PSCB)
    {
        //检查PSCB的硬件平台，版本号高位中 '1’: TMS570 , '2': STM407
        if (hardware_version == TMS570)
        {
            //检查PSCB版本，如果PSCB版本号低于03_01则进行报错
            if (scb_version < 301)
            {
                ret = -1;
            }
        }
        else if (hardware_version == STM407)
        {
            //检查PSCB版本，如果PSCB版本号低于13_08则进行报错
            if (scb_version < 1308)
            {
                ret = -1;
            }
        }
        if (-1 == ret)
        {
            // reportError(ERROR_SCB_VERSION_IS_INCORRECT, "The SCB version is too low to fully support all safety features!\n");
        }
        return ret;
    }
    else if (cab_internal_info.p_scb_type == SCB)
    {
        return ret;
    }
    return 0;
}

static void ini_rc_version_data()
{
    int version[8] = {0};
    char text[] = "";                //PACKAGE_VERSION;
    const char delimiter[3] = "._";  // 分隔符为 . 和 _

    char* token;
    int numbers[4];
    int i = 0;

    token = strtok(text, delimiter);

    while (token != NULL && i < 4)
    {
        numbers[i++] = atoi(token);
        token = strtok(NULL, delimiter);
    }

    unsigned int version_data = 0;
    for (i = 0; i < 4; i++) { version_data = (version_data << 8) + numbers[i]; }
    cab_internal_info.rc_version = version_data;
}

static void send_rc_version_to_scb()
{
    //目前只有PSCB才会增发控制器版本号
    if (cab_internal_info.p_scb_type != PSCB)
    {
        return;
    }
    static int scb_set_rc_version_timeout_cnt = 0;
    if (cab_internal_info.set_common_config_req.RC_version_param == 1)
    {
        scb_set_rc_version_timeout_cnt++;
        if (scb_set_rc_version_timeout_cnt % 10 == 1)
        {
            if (0xFFFF0000 == zuc_set_scb_para(index_316B[cab_type], 0xAA, cab_internal_info.rc_version))
            {
                scb_set_rc_version_timeout_cnt = scb_set_rc_version_timeout_cnt - 1;
            }
        }
        if (scb_set_rc_version_timeout_cnt >= SCB_RANDOM_INDEX_TIMEOUT)
        {
            cab_internal_info.set_common_config_req.RC_version_param = -1;
            scb_set_rc_version_timeout_cnt = 0;
            // reportError(ERROR_SEND_RC_VERSION_TO_SCB_FAILED, "Send the RC version information failed!\n");
            // rtapi_print("Send the RC version information failed!\n");
        }
    }
    else
    {
        scb_set_rc_version_timeout_cnt = 0;
    }
}

void set_scb_safety_DI_param(unsigned int safety_di_cfg) {}

static void send_safety_DI_to_scb() {}

void set_scb_safety_DO_param(unsigned int safety_do_cfg) {}

static void send_safety_DO_to_scb() {}

//暂时停用了，没有使用SCB对功率的限制功能
void set_scb_power_limit_param(float power_limit) {}
//暂时停用了，没有使用SCB对功率的限制功能
static void send_robot_power_limit_to_scb() {}

void set_scb_TCP_offset_param(double x, double y, double z) {}

static void send_tcp_offset_to_scb() {}

void set_scb_cab_id_param(unsigned int cabinet_id) {}

static void send_cab_id_to_scb() {}

void set_scb_cab_voltage_param(unsigned int cab_voltage) {}

static void send_cab_voltage_to_scb() {}

void set_scb_drag_tcp_speed_limit_param(unsigned int drag_tcp_speed_limit) {}

static void send_drag_tcp_speed_limit_to_SCB() {}

/*    
* query scb information 
* 每200个周期(1.6s)请求一次SCB信息
*/
static void zuc_query_scb_info(void) {}

/*请求确认是SCB还是PSCB*/
static void zuc_query_p_scb_type() {}

/**
 * 0x612报文随机索引发送内容处理
 * 向SCB请求数据或者向SCB发送配置信息
*/
static void zuc_random_index_to_scb()
{
    zuc_query_p_scb_type();
    // -1 is default value
    if (cab_internal_info.p_scb_type == -1)
    {
        //如果识别不到硬件型号所有随机索引都不进行发送
        return;
    }
    /* if bus off clear query bussy status */
    if (cab_internal_info.inter_can_bsof_sts == 1)
    {
        zuc_inter_can_msg_sts[ZUC_INTER_CAN_0x612].tx_en_flag = 0;
    }

    check_scb_version();
    send_rc_version_to_scb();
    send_safety_DI_to_scb();
    send_safety_DO_to_scb();
    send_robot_power_limit_to_scb();
    send_tcp_offset_to_scb();
    send_cab_id_to_scb();
    send_cab_voltage_to_scb();
    send_drag_tcp_speed_limit_to_SCB();
    /*
    * update scb information
    * 每200个周期(1.6s)向SCB请求一次信息
    * p is pscb  0 is scb
    */
    zuc_query_scb_info();
}

static void zuc_inter_signal_manage(void)
{
    /*do data*/
    memcpy((void*)(zuc_internal_comm_send_msg[1].Data), (void*)(cab_internal_info.scb_do_ctl.msg_data), 8);
    /*ao data*/
    memcpy((void*)(zuc_internal_comm_send_msg[2].Data), (void*)(cab_internal_info.scb_ao_ctl.msg_data), 8);
    //safety do data
    memcpy((void*)(zuc_internal_comm_send_msg[3].Data), (void*)(cab_internal_info.scb_safety_do_ctl.msg_data), 8);

    //处理需要随机索引发送的内容
    zuc_random_index_to_scb();
}

// void scb_stop_comm()
// {
// 	comm_stop_flag = 1;
// }
/**
 * @brief 
 * 	static cariable for debug usage
 */
// static unsigned char local_sfgrd_rst = 0;;
// static unsigned char local_reduce = 0;
// static unsigned char local_add_sfgrd_stop= 0;
// static unsigned char local_add_estop= 0;
// /**
//  * @brief
//  * 	debug interface for signal monitor
//  */
// static void debug_show_safety_IOsts(void)
// {
// 	if(cab_internal_info.scb_di_uhi_info.di_signal.local_add_estop == local_add_estop)
// 	{}
// 	else{
// 		printf("cab_internal_info.scb_di_uhi_info.di_signal.local_add_estop = %d\n",cab_internal_info.scb_di_uhi_info.di_signal.local_add_estop);
// 		local_add_estop = cab_internal_info.scb_di_uhi_info.di_signal.local_add_estop;
// 	}

// 	if(cab_internal_info.scb_di_uhi_info.di_signal.local_add_sfgrd_stop == local_add_sfgrd_stop)
// 	{}
// 	else{
// 		printf("cab_internal_info.scb_di_uhi_info.di_signal.local_add_sfgrd_stop = %d\n",cab_internal_info.scb_di_uhi_info.di_signal.local_add_sfgrd_stop);
// 		local_add_sfgrd_stop = cab_internal_info.scb_di_uhi_info.di_signal.local_add_sfgrd_stop;
// 	}

// 	if(cab_internal_info.scb_di_uhi_info.di_signal.local_add_sfgrd_stop == local_add_sfgrd_stop)
// 	{}
// 	else{
// 		printf("cab_internal_info.scb_di_uhi_info.di_signal.local_add_sfgrd_stop = %d\n",cab_internal_info.scb_di_uhi_info.di_signal.local_add_sfgrd_stop);
// 		local_add_sfgrd_stop = cab_internal_info.scb_di_uhi_info.di_signal.local_add_sfgrd_stop;
// 	}

// 	if(cab_internal_info.scb_di_uhi_info.di_signal.local_reduce == local_reduce)
// 	{}
// 	else{
// 		printf("cab_internal_info.scb_di_uhi_info.di_signal.local_reduce = %d\n",cab_internal_info.scb_di_uhi_info.di_signal.local_reduce);
// 		local_reduce = cab_internal_info.scb_di_uhi_info.di_signal.local_reduce;
// 	}
// 	if(cab_internal_info.scb_di_uhi_info.di_signal.local_sfgrd_rst == local_sfgrd_rst)
// 	{}
// 	else{
// 		printf("cab_internal_info.scb_di_uhi_info.di_signal.local_sfgrd_rst = %d\n",cab_internal_info.scb_di_uhi_info.di_signal.local_sfgrd_rst);
// 		local_sfgrd_rst = cab_internal_info.scb_di_uhi_info.di_signal.local_sfgrd_rst;
// 	}
// }
//
void scb_comm_init(int scb_cab_type)
{
    cab_type = (scb_cab_type == 4) ? (1) : (0);  // 4 for cabv3
    // cab_internal_info.button_panel_control.bp_rgb_lock_color = ZUC_BP_RGB_YELLOW;
    // cab_internal_info.button_panel_control.bp_rgb_lock_brightness = 50;
    cab_internal_info.p_scb_type = -1;
    cab_internal_info.set_safety_config_req.safetyDI_param = -1;
    cab_internal_info.set_safety_config_req.safetyDO_param = -1;
    cab_internal_info.set_safety_config_req.robot_power_limit_param = -1;
    cab_internal_info.set_common_config_req.cab_id_param = -1;
    cab_internal_info.set_common_config_req.cab_voltage_param = -1;
    cab_internal_info.set_common_config_req.TCP_offset_param = -1;
    cab_internal_info.set_safety_config_req.drag_tcp_speed_limit_param = -1;
    //初始值为1，在BP CAN建立起来确认硬件型号以后向SCB/PSCB发送
    cab_internal_info.set_common_config_req.RC_version_param = 1;
    ini_rc_version_data();
}

//jkzuc -s 的调试仿真
void scb_can_simulation()
{
    if (zucmotConfig->cab_type == 2)
    {
        cab_internal_info.scb_di_uhi_info.di_signal.local_bp_estop_1 = 1;
        cab_internal_info.scb_di_uhi_info.di_signal.local_bp_estop_2 = 1;
        cab_internal_info.scb_di_uhi_info.di_signal.local_usr_estop_1 = 1;
        cab_internal_info.scb_di_uhi_info.di_signal.local_usr_estop_2 = 1;
        cab_internal_info.scb_di_uhi_info.di_signal.local_usr_stop_1 = 1;
        cab_internal_info.scb_di_uhi_info.di_signal.local_usr_stop_2 = 1;
        cab_internal_info.scb_do_ao_feedback_info.msg_data[0] = cab_internal_info.scb_do_ctl.msg_data[0];
        cab_internal_info.scb_do_ao_feedback_info.msg_data[1] = cab_internal_info.scb_do_ctl.msg_data[1];
    }
    else if (zucmotConfig->cab_type == 3)
    {
        cab_internal_info.scb_di_uhi_info.pscb_di_signal.local_bp_estop_1 = 1;
        cab_internal_info.scb_di_uhi_info.pscb_di_signal.local_bp_estop_2 = 1;
        cab_internal_info.scb_di_uhi_info.pscb_di_signal.local_usr_estop_1 = 1;
        cab_internal_info.scb_di_uhi_info.pscb_di_signal.local_usr_estop_2 = 1;
        cab_internal_info.scb_di_uhi_info.pscb_di_signal.local_usr_stop_1 = 1;
        cab_internal_info.scb_di_uhi_info.pscb_di_signal.local_usr_stop_2 = 1;
        cab_internal_info.scb_do_ao_feedback_info.msg_data[0] = cab_internal_info.scb_do_ctl.msg_data[0];
        cab_internal_info.scb_ai_uhi_info.pscb_ai_signal.dio_type = cab_internal_info.scb_ao_ctl.pscb_ao_signal.user_dio_cfg;
    }
}

// receive thread
void scb_can_comm(void* arg)
{
    if (zucmotConfig->safetyCfg.motRunMode == RUN_MODE_UNSPECIFIED)
    {
        static int __flag = 1;
        if (__flag)
        {
            __flag = 0;
            rtapi_print("scb_can simulation\n");
        }
    }
    else if (zucmotConfig->safetyCfg.motRunMode == RUN_MODE_SIM_WITHOUT_CAN)
    {
        scb_can_simulation();
    }
    else
    {
        /*analysis recv messages*/
        if (zuc_inter_comm_mesg_rx() == SCB_CAN_STA_INIT)
        {
            return;
        }

        if (zucmotStatus->is_set_scb_br_start_vol_flag == 1)
        {
            zucmotStatus->after_set_scb_br_start_vol_flag++;
        }
        /*package and data management
		* then send the message
		*/
        zuc_inter_signal_manage();
        zuc_inter_comm_mesg_tx();
        /*connection management
		*/
        if (zucmotStatus->set_scb_br_start_vol_flag == 1)
        {
            zucmotStatus->is_set_scb_br_start_vol_flag = 1;
        }
        zuc_inter_comm_manage();
        /*debug operations*/
        zuc_inter_comm_debug_show_infor();
    }
}
